/*
 * @Description: 
 * @Author: Ren Qian
 * @Date: 2020-03-01 18:07:42
 */

#include "lidar_localization/models/graph_optimizer/g2o/g2o_graph_optimizer.hpp"
#include "glog/logging.h"
#include "lidar_localization/tools/tic_toc.hpp"

namespace lidar_localization
{
    G2oGraphOptimizer::G2oGraphOptimizer(const std::string &solver_type)
    {
        graph_ptr_.reset(new g2o::SparseOptimizer());

        g2o::OptimizationAlgorithmFactory *solver_factory = g2o::OptimizationAlgorithmFactory::instance();
        g2o::OptimizationAlgorithmProperty solver_property;
        g2o::OptimizationAlgorithm *solver = solver_factory->construct(solver_type, solver_property);
        graph_ptr_->setAlgorithm(solver);

        if (!graph_ptr_->solver())
        {
            LOG(ERROR) << "G2O 优化器创建失败！";
        }
        robust_kernel_factory_ = g2o::RobustKernelFactory::instance();
        state_num_ = 2;
        need_robust_kernel_ = false;
    }

    bool G2oGraphOptimizer::Optimize()
    {
        static int optimize_cnt = 0;
        if (graph_ptr_->edges().size() < 1)
        {
            return false;
        }

        TicToc optimize_time;
        graph_ptr_->initializeOptimization();
        graph_ptr_->computeInitialGuess();
        graph_ptr_->computeActiveErrors();
        graph_ptr_->setVerbose(false);

        double chi2 = graph_ptr_->chi2();
        int iterations = graph_ptr_->optimize(max_iterations_num_);

        LOG(INFO) << std::endl
                  << "------ 完成第 " << ++optimize_cnt << " 次后端优化 -------" << std::endl
                  << "顶点数：" << graph_ptr_->vertices().size() << ", 边数： " << graph_ptr_->edges().size() << std::endl
                  << "迭代次数： " << iterations << "/" << max_iterations_num_ << std::endl
                  << "用时：" << optimize_time.toc() << std::endl
                  << "优化前后误差变化：" << chi2 << "--->" << graph_ptr_->chi2()
                  << std::endl
                  << std::endl;

        return true;
    }

    bool G2oGraphOptimizer::GetOptimizedPose(std::deque<Eigen::Matrix4f> &optimized_pose)
    {
        optimized_pose.clear();
        int vertex_num = graph_ptr_->vertices().size();

        for (int i = 0; i < vertex_num / state_num_; i++)
        {
            g2o::VertexVec *vp = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(i * state_num_));
            Eigen::Vector3d p = vp->estimate();
            g2o::VertexQ *vq = dynamic_cast<g2o::VertexQ *>(graph_ptr_->vertex(i * state_num_ + 1));
            Eigen::Quaterniond q = vq->estimate();

            Eigen::Isometry3d pose = Eigen::Isometry3d::Identity();
            pose.linear() = q.toRotationMatrix();
            pose.translation() = p;
            optimized_pose.push_back(pose.matrix().cast<float>());
            if (state_num_ == 5)
            {
                Eigen::Vector3d v = (dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(i * state_num_ + 2)))->estimate();
                Eigen::Vector3d ba = (dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(i * state_num_ + 3)))->estimate();
                Eigen::Vector3d bg = (dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(i * state_num_ + 4)))->estimate();
            }
        }
        return true;
    }

    int G2oGraphOptimizer::GetNodeNum()
    {
        return graph_ptr_->vertices().size();
    }

    void G2oGraphOptimizer::AddSe3Node(const Eigen::Isometry3d &pose, bool need_fix)
    {
        g2o::VertexSE3 *vertex(new g2o::VertexSE3());
        vertex->setId(graph_ptr_->vertices().size());
        vertex->setEstimate(pose);
        if (need_fix)
        {
            vertex->setFixed(true);
        }
        graph_ptr_->addVertex(vertex);
    }

    void G2oGraphOptimizer::SetEdgeRobustKernel(std::string robust_kernel_name,
                                                double robust_kernel_size)
    {
        robust_kernel_name_ = robust_kernel_name;
        robust_kernel_size_ = robust_kernel_size;
        need_robust_kernel_ = true;
    }

    void G2oGraphOptimizer::AddSe3Edge(int vertex_index1,
                                       int vertex_index2,
                                       const Eigen::Isometry3d &relative_pose,
                                       const Eigen::VectorXd noise)
    {
        Eigen::MatrixXd information_matrix = CalculateSe3EdgeInformationMatrix(noise);
        g2o::VertexSE3 *v1 = dynamic_cast<g2o::VertexSE3 *>(graph_ptr_->vertex(vertex_index1));
        g2o::VertexSE3 *v2 = dynamic_cast<g2o::VertexSE3 *>(graph_ptr_->vertex(vertex_index2));
        g2o::EdgeSE3 *edge(new g2o::EdgeSE3());
        edge->setMeasurement(relative_pose);
        edge->setInformation(information_matrix);
        edge->vertices()[0] = v1;
        edge->vertices()[1] = v2;
        graph_ptr_->addEdge(edge);
        if (need_robust_kernel_)
        {
            AddRobustKernel(edge, robust_kernel_name_, robust_kernel_size_);
        }
    }

    Eigen::MatrixXd G2oGraphOptimizer::CalculateSe3EdgeInformationMatrix(Eigen::VectorXd noise)
    {
        Eigen::MatrixXd information_matrix = Eigen::MatrixXd::Identity(6, 6);
        information_matrix = CalculateDiagMatrix(noise);
        return information_matrix;
    }

    void G2oGraphOptimizer::AddRobustKernel(g2o::OptimizableGraph::Edge *edge, const std::string &kernel_type, double kernel_size)
    {
        if (kernel_type == "NONE")
        {
            return;
        }

        g2o::RobustKernel *kernel = robust_kernel_factory_->construct(kernel_type);
        if (kernel == nullptr)
        {
            std::cerr << "warning : invalid robust kernel type: " << kernel_type << std::endl;
            return;
        }

        kernel->setDelta(kernel_size);
        edge->setRobustKernel(kernel);
    }

    Eigen::MatrixXd G2oGraphOptimizer::CalculateDiagMatrix(Eigen::VectorXd noise)
    {
        Eigen::MatrixXd information_matrix = Eigen::MatrixXd::Identity(noise.rows(), noise.rows());
        for (int i = 0; i < noise.rows(); i++)
        {
            information_matrix(i, i) /= noise(i);
        }
        return information_matrix;
    }

    void G2oGraphOptimizer::AddSe3PriorXYZEdge(int se3_vertex_index,
                                               const Eigen::Vector3d &xyz,
                                               Eigen::VectorXd noise)
    {
        Eigen::MatrixXd information_matrix = CalculateDiagMatrix(noise);
        g2o::VertexSE3 *v_se3 = dynamic_cast<g2o::VertexSE3 *>(graph_ptr_->vertex(se3_vertex_index));
        g2o::EdgeSE3PriorXYZ *edge(new g2o::EdgeSE3PriorXYZ());
        edge->setMeasurement(xyz);
        edge->setInformation(information_matrix);
        edge->vertices()[0] = v_se3;
        graph_ptr_->addEdge(edge);
    }

    void G2oGraphOptimizer::AddSe3PriorQuaternionEdge(int se3_vertex_index,
                                                      const Eigen::Quaterniond &quat,
                                                      Eigen::VectorXd noise)
    {
        Eigen::MatrixXd information_matrix = CalculateSe3PriorQuaternionEdgeInformationMatrix(noise);
        g2o::VertexSE3 *v_se3 = dynamic_cast<g2o::VertexSE3 *>(graph_ptr_->vertex(se3_vertex_index));
        g2o::EdgeSE3PriorQuat *edge(new g2o::EdgeSE3PriorQuat());
        edge->setMeasurement(quat);
        edge->setInformation(information_matrix);
        edge->vertices()[0] = v_se3;
        graph_ptr_->addEdge(edge);
    }

    // TODO: 姿态观测的信息矩阵尚未添加
    // 备注：各位使用时可只用位置观测，而不用姿态观测，影响不大
    // 我自己在别的地方尝试过增加姿态观测，但效果反而变差，如果感兴趣，可自己编写此处的信息矩阵，并在后端优化中添加相应的边进行验证
    Eigen::MatrixXd G2oGraphOptimizer::CalculateSe3PriorQuaternionEdgeInformationMatrix(Eigen::VectorXd noise)
    {
        Eigen::MatrixXd information_matrix;
        return information_matrix;
    }

    void G2oGraphOptimizer::AddNode(const Eigen::Vector3d &p, const Eigen::Quaterniond &q, const Eigen::Vector3d &v, const Eigen::Vector3d &ba, const Eigen::Vector3d &bg, bool need_fix)
    {
        state_num_ = 5;
        g2o::VertexVec *vp(new g2o::VertexVec());
        vp->setId(graph_ptr_->vertices().size());
        vp->setEstimate(p);
        if (need_fix)
        {
            vp->setFixed(true);
        }
        graph_ptr_->addVertex(vp);

        g2o::VertexQ *vq(new g2o::VertexQ());
        vq->setId(graph_ptr_->vertices().size());
        vq->setEstimate(q);
        if (need_fix)
        {
            vq->setFixed(true);
        }
        graph_ptr_->addVertex(vq);

        g2o::VertexVec *vv(new g2o::VertexVec());
        vv->setId(graph_ptr_->vertices().size());
        vv->setEstimate(v);
        graph_ptr_->addVertex(vv);

        g2o::VertexVec *vba(new g2o::VertexVec());
        vba->setId(graph_ptr_->vertices().size());
        vba->setEstimate(ba);
        graph_ptr_->addVertex(vba);

        g2o::VertexVec *vbg(new g2o::VertexVec());
        vbg->setId(graph_ptr_->vertices().size());
        vbg->setEstimate(bg);
        graph_ptr_->addVertex(vbg);
    }
    void G2oGraphOptimizer::AddNode(const Eigen::Vector3d &p, const Eigen::Quaterniond &q, bool need_fix)
    {
        state_num_ = 2;
        g2o::VertexVec *vp(new g2o::VertexVec());
        vp->setId(graph_ptr_->vertices().size());
        vp->setEstimate(p);
        if (need_fix)
        {
            vp->setFixed(true);
        }
        graph_ptr_->addVertex(vp);

        g2o::VertexQ *vq(new g2o::VertexQ());
        vq->setId(graph_ptr_->vertices().size());
        vq->setEstimate(q);
        if (need_fix)
        {
            vq->setFixed(true);
        }
        graph_ptr_->addVertex(vq);
    }
    void G2oGraphOptimizer::AddEdgeGnss(int vertex_index, const Eigen::Vector3d &p, Eigen::VectorXd noise)
    {
        Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Zero();
        for (unsigned int i = 0; i < 3; i++)
        {
            information_matrix(i, i) = 1.0 / noise(i) / noise(i);
        }
        g2o::VertexVec *vp = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index));
        g2o::EdgeGnss *edge(new g2o::EdgeGnss());
        edge->setMeasurement(p);
        edge->setInformation(information_matrix);
        edge->vertices()[0] = vp;
        graph_ptr_->addEdge(edge);
    }
    void G2oGraphOptimizer::AddEdgeMatch(int vertex_index, const Eigen::Vector3d &p, const Eigen::Quaterniond &q, Eigen::VectorXd noise)
    {
        Eigen::Matrix<double, 6, 6> information_matrix = Eigen::Matrix<double, 6, 6>::Zero();
        for (unsigned int i = 0; i < 6; i++)
        {
            information_matrix(i, i) = 1.0 / noise(i) / noise(i);
        }
        g2o::VertexVec *vp = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index));
        g2o::VertexQ *vq = dynamic_cast<g2o::VertexQ *>(graph_ptr_->vertex(vertex_index + 1));
        g2o::EdgeMatch *edge(new g2o::EdgeMatch());
        edge->setMeasurement(std::pair<Eigen::Vector3d, Eigen::Quaterniond>(p, q));
        edge->setInformation(information_matrix);
        edge->vertices()[0] = vp;
        edge->vertices()[1] = vq;
        graph_ptr_->addEdge(edge);
    }
    void G2oGraphOptimizer::AddEdgeOdom(int vertex_index1, int vertex_index2, const Eigen::Vector3d &pij, const Eigen::Quaterniond &qij, Eigen::VectorXd noise)
    {
        Eigen::Matrix<double, 6, 6> information_matrix = Eigen::Matrix<double, 6, 6>::Zero();
        for (unsigned int i = 0; i < 6; i++)
        {
            information_matrix(i, i) = 1.0 / noise(i) / noise(i);
        }
        g2o::VertexVec *vp1 = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index1));
        g2o::VertexQ *vq1 = dynamic_cast<g2o::VertexQ *>(graph_ptr_->vertex(vertex_index1 + 1));
        g2o::VertexVec *vp2 = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index2));
        g2o::VertexQ *vq2 = dynamic_cast<g2o::VertexQ *>(graph_ptr_->vertex(vertex_index2 + 1));
        g2o::EdgeOdom *edge(new g2o::EdgeOdom());
        edge->setMeasurement(std::pair<Eigen::Vector3d, Eigen::Quaterniond>(pij, qij));
        edge->setInformation(information_matrix);
        edge->vertices()[0] = vp1;
        edge->vertices()[1] = vq1;
        edge->vertices()[2] = vp2;
        edge->vertices()[3] = vq2;
        graph_ptr_->addEdge(edge);
        if (need_robust_kernel_)
        {
            AddRobustKernel(edge, robust_kernel_name_, robust_kernel_size_);
        }
    }
    void G2oGraphOptimizer::AddEdgeImu(int vertex_index1, int vertex_index2, const std::shared_ptr<PreIntegration> &pre_integration)
    {
        g2o::VertexVec *vp1 = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index1));
        g2o::VertexQ *vq1 = dynamic_cast<g2o::VertexQ *>(graph_ptr_->vertex(vertex_index1 + 1));
        g2o::VertexVec *vv1 = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index1 + 2));
        g2o::VertexVec *vba1 = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index1 + 3));
        g2o::VertexVec *vbg1 = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index1 + 4));

        g2o::VertexVec *vp2 = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index2));
        g2o::VertexQ *vq2 = dynamic_cast<g2o::VertexQ *>(graph_ptr_->vertex(vertex_index2 + 1));
        g2o::VertexVec *vv2 = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index2 + 2));
        g2o::VertexVec *vba2 = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index2 + 3));
        g2o::VertexVec *vbg2 = dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index2 + 4));

        g2o::EdgeImu *edge(new g2o::EdgeImu());
        edge->setMeasurement(pre_integration);
        edge->vertices()[0] = vp1;
        edge->vertices()[1] = vq1;
        edge->vertices()[2] = vv1;
        edge->vertices()[3] = vba1;
        edge->vertices()[4] = vbg1;
        edge->vertices()[5] = vp2;
        edge->vertices()[6] = vq2;
        edge->vertices()[7] = vv2;
        edge->vertices()[8] = vba2;
        edge->vertices()[9] = vbg2;
        graph_ptr_->addEdge(edge);
        if (need_robust_kernel_)
        {
            AddRobustKernel(edge, robust_kernel_name_, robust_kernel_size_);
        }
    }
    State G2oGraphOptimizer::GetState(int vertex_index)
    {
        State state;
        state.p = (dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index)))->estimate();
        state.q = (dynamic_cast<g2o::VertexQ *>(graph_ptr_->vertex(vertex_index + 1)))->estimate();
        if (state_num_ == 5)
        {
            state.v = (dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index + 2)))->estimate();
            state.ba = (dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index + 3)))->estimate();
            state.bg = (dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertex(vertex_index + 4)))->estimate();
        }
        return state;
    }
    void G2oGraphOptimizer::DeleteNewestState()
    {
        int index = graph_ptr_->vertices().size();
        for (unsigned int i = 1; i < 6; ++i)
        {
            graph_ptr_->removeVertex(graph_ptr_->vertex(index - i), false);
        }
    }
    void G2oGraphOptimizer::Marginalize()
    {
        std::vector<g2o::HyperGraph::Edge *> margedges;
        std::vector<int> margvertices;
        for (int i = 0; i < 5; ++i)
        {
            margvertices.push_back(i);
        }
        for (auto it = graph_ptr_->edges().begin(); it != graph_ptr_->edges().end(); ++it)
        {
            auto fi = std::find_if((*it)->vertices().begin(), (*it)->vertices().end(), [](const std::vector<g2o::HyperGraph::Vertex *>::value_type &v) { return v->id() < 5; });
            if (fi != (*it)->vertices().end())
            {
                margedges.push_back(*it);
                for (auto vit = (*it)->vertices().begin(); vit != (*it)->vertices().end(); ++vit)
                {
                    int vid = (*vit)->id();
                    auto mv = std::find(margvertices.begin(), margvertices.end(), vid);
                    if (mv == margvertices.end())
                    {
                        margvertices.push_back(vid);
                    }
                }
            }
        }
        int m = 15;
        int pos = margvertices.size() * 3;
        int n = pos - m;
        Eigen::MatrixXd A(pos, pos);
        Eigen::VectorXd b(pos);
        A.setZero();
        b.setZero();
        for (auto it = margedges.begin(); it != margedges.end(); ++it)
        {
            std::vector<g2o::MatrixX::MapType, Eigen::aligned_allocator<g2o::MatrixX::MapType>> jacs;
            Eigen::MatrixXd info;
            Eigen::VectorXd error;
            if (dynamic_cast<g2o::EdgeGnss *>(*it))
            {
                (dynamic_cast<g2o::EdgeGnss *>(*it))->computeError();
                (dynamic_cast<g2o::EdgeGnss *>(*it))->linearizeOplus();
                jacs = (dynamic_cast<g2o::EdgeGnss *>(*it))->GetJacobian();
                info = (dynamic_cast<g2o::EdgeGnss *>(*it))->information();
                error = (dynamic_cast<g2o::EdgeGnss *>(*it))->error();
            }
            else if (dynamic_cast<g2o::EdgeImu *>(*it))
            {
                (dynamic_cast<g2o::EdgeImu *>(*it))->computeError();
                (dynamic_cast<g2o::EdgeImu *>(*it))->linearizeOplus();
                jacs = (dynamic_cast<g2o::EdgeImu *>(*it))->GetJacobian();
                info = (dynamic_cast<g2o::EdgeImu *>(*it))->information();
                error = (dynamic_cast<g2o::EdgeImu *>(*it))->error();
            }
            else if (dynamic_cast<g2o::EdgeMatch *>(*it))
            {
                (dynamic_cast<g2o::EdgeMatch *>(*it))->computeError();
                (dynamic_cast<g2o::EdgeMatch *>(*it))->linearizeOplus();
                jacs = (dynamic_cast<g2o::EdgeMatch *>(*it))->GetJacobian();
                info = (dynamic_cast<g2o::EdgeMatch *>(*it))->information();
                error = (dynamic_cast<g2o::EdgeMatch *>(*it))->error();
            }
            else if (dynamic_cast<g2o::EdgeOdom *>(*it))
            {
                (dynamic_cast<g2o::EdgeOdom *>(*it))->computeError();
                (dynamic_cast<g2o::EdgeOdom *>(*it))->linearizeOplus();
                jacs = (dynamic_cast<g2o::EdgeOdom *>(*it))->GetJacobian();
                info = (dynamic_cast<g2o::EdgeOdom *>(*it))->information();
                error = (dynamic_cast<g2o::EdgeOdom *>(*it))->error();
            }
            else if (dynamic_cast<g2o::EdgeMarginalization *>(*it))
            {
                (dynamic_cast<g2o::EdgeMarginalization *>(*it))->computeError();
                (dynamic_cast<g2o::EdgeMarginalization *>(*it))->linearizeOplus();
                jacs = (dynamic_cast<g2o::EdgeMarginalization *>(*it))->GetJacobian();
                info = (dynamic_cast<g2o::EdgeMarginalization *>(*it))->information();
                error = (dynamic_cast<g2o::EdgeMarginalization *>(*it))->error();
            }
            else
            {
                std::cerr << "Edge Error!!!" << std::endl;
                return;
            }

            std::vector<g2o::HyperGraph::Vertex *> vertices((*it)->vertices());
            for (unsigned int i = 0; i < vertices.size(); ++i)
            {
                int vid = vertices[i]->id();
                auto vit = std::find(margvertices.begin(), margvertices.end(), vid);
                if (vit == margvertices.end())
                {
                    std::cerr << "vit error!!!" << std::endl;
                    return;
                }
                int index_i = std::distance(margvertices.begin(), vit);
                for (unsigned int j = i; j < vertices.size(); ++j)
                {
                    vid = vertices[j]->id();
                    vit = std::find(margvertices.begin(), margvertices.end(), vid);
                    if (vit == margvertices.end())
                    {
                        std::cerr << "vit error!!!" << std::endl;
                        return;
                    }
                    int index_j = std::distance(margvertices.begin(), vit);
                    if (i == j)
                    {
                        A.block(index_i * 3, index_j * 3, 3, 3) += jacs[i].transpose() * info * jacs[j];
                    }
                    else
                    {
                        A.block(index_i * 3, index_j * 3, 3, 3) += jacs[i].transpose() * info * jacs[j];
                        A.block(index_j * 3, index_i * 3, 3, 3) = A.block(index_i * 3, index_j * 3, 3, 3).transpose();
                    }
                }
                b.segment(index_i * 3, 3) += jacs[i].transpose() * info * error;
            }
        }

        Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose());
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);
        double eps = 1e-8;
        Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();

        Eigen::VectorXd bmm = b.segment(0, m);
        Eigen::MatrixXd Amr = A.block(0, m, m, n);
        Eigen::MatrixXd Arm = A.block(m, 0, n, m);
        Eigen::MatrixXd Arr = A.block(m, m, n, n);
        Eigen::VectorXd brr = b.segment(m, n);
        A = Arr - Arm * Amm_inv * Amr;
        b = brr - Arm * Amm_inv * bmm;

        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);
        Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));
        Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));

        Eigen::VectorXd S_sqrt = S.cwiseSqrt();
        Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();

        Eigen::MatrixXd linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
        Eigen::VectorXd linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;

        g2o::EdgeMarginalization *edge(new g2o::EdgeMarginalization());
        edge->setSize(linearized_residuals.rows() / 3);
        edge->setMeasurement(std::make_pair(linearized_jacobians, linearized_residuals));
        for (int i = 5; i < margvertices.size(); ++i)
        {
            if (dynamic_cast<g2o::VertexQ *>(graph_ptr_->vertices().at(margvertices[i])))
            {
                edge->push_back(i - 5, (dynamic_cast<g2o::VertexQ *>(graph_ptr_->vertices().at(margvertices[i])))->estimate());
            }
            else if (dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertices().at(margvertices[i])))
            {
                edge->push_back(i - 5, (dynamic_cast<g2o::VertexVec *>(graph_ptr_->vertices().at(margvertices[i])))->estimate());
            }
            else
            {
                std::cerr << "Vertex Error!!!" << std::endl;
                return;
            }
            edge->vertices()[i - 5] = graph_ptr_->vertices().at(margvertices[i]);
        }

        DeleteOldestState();

        graph_ptr_->addEdge(edge);
        if (need_robust_kernel_)
        {
            AddRobustKernel(edge, robust_kernel_name_, robust_kernel_size_);
        }

        SlideWindow();
    }
    void G2oGraphOptimizer::DeleteOldestState()
    {
        for (unsigned int i = 0; i < 5; ++i)
        {
            graph_ptr_->removeVertex(graph_ptr_->vertex(i), false);
        }
    }

    void G2oGraphOptimizer::SlideWindow()
    {
        for (unsigned int i = 0; i < graph_ptr_->vertices().size(); ++i)
        {
            graph_ptr_->changeId(graph_ptr_->vertex(i + 5), i);
        }
    }
} // namespace lidar_localization
